clear all; close all;

samples = Samples('E5b', '~/wnav/snapshots/snapshot_mem_02_e5b.dat');
E5bI_sc = SecondaryPRNCode('E5bI');
E5bQ_sc = SecondaryPRNCode('E5bQ');
NumberOfSamples = floor(E5bI_sc.ChipPeriod/samples.SamplingPeriod);
NumberOfPeriods = 1;
E5bI_pc = PrimaryPRNCode('E5bI', NumberOfSamples, 1); 
E5bQ_pc = PrimaryPRNCode('E5bQ', NumberOfSamples, 1); 
samples.loadSamples(NumberOfSamples*(NumberOfPeriods+1));

% Acquisition
acq = Acquisition('E5b', samples.FrequencyOffset);
SVID = 12;
fprintf(1, 'Acquiring E5b PRN %d ... \n', SVID);
acq.Acquire(samples, NumberOfPeriods, 1e-5, SVID, E5bI_pc, E5bQ_pc);
if (acq.DetectionStatus)
	fprintf(1, 'E5b PRN %d successfully acquired! \n', SVID);
	figure(SVID); clf;
	[m n] = size(acq.CCF);
	plot((1:m)*samples.SamplingPeriod, abs(acq.CCF),...
			 (1:m)*samples.SamplingPeriod, repmat(acq.Threshold, m, 1));
	xlabel('Time [s]');
	ylabel('CorrOut [-]');
	legend('CCF','Threshold');
end

% Tracking
if (acq.DetectionStatus)
	
	tracking = Tracking('E5b', SVID, samples, E5bI_pc, E5bQ_pc);
	LoopOrderFrequencyCoarse = 1;
	BandwidthFrequencyCoarse = 5.0;
	LoopOrderTimeCoarse = 2;
	BandwidthTimeCoarse = 30.0;
	LoopOrderFrequencyFine = 1;
	BandwidthFrequencyFine = 5.0;
	LoopOrderTimeFine = 2;
	BandwidthTimeFine = 10.0;
	LoopOrderCarrierPhaseFine = 1;
	BandwidthCarrierPhaseFine = 2.0;
	tracking.setFrequencyShiftFilter(LoopOrderFrequencyCoarse, ...
																	 BandwidthFrequencyCoarse);
	tracking.setTimeShiftFilter(LoopOrderTimeCoarse, ...
															BandwidthTimeCoarse);
	tracking.initState(acq.TimeShift, acq.FrequencyShift);

	NumberOfCorrelations = 2900;
	TimeShiftMem = zeros(1, NumberOfCorrelations);
	FrequencyShiftMem = zeros(1, NumberOfCorrelations);
	CarrierPhaseShiftMem = zeros(1, NumberOfCorrelations);
	[m n] = size(tracking.CorrelatorOut);
	CorrelatorOutMem = zeros(n, NumberOfCorrelations);

	bitsync = BitSynchronization('E5b', SVID, 'BitSyncE5b_tb.txt');
	ThresholdMem = zeros(n/3, NumberOfCorrelations);
	SuffStatMem = zeros(n/3, NumberOfCorrelations);

	for i=1:NumberOfCorrelations
		fprintf(1, 'Tracking number of correlation = %d \n', i);

		tracking.Track();

		bitsync.addToBuffer([tracking.CorrelatorOut(2) ...
												 tracking.CorrelatorOut(5)].', ...
	 											 tracking.FrequencyShift,...
												 tracking.TimeShift, ...
												 tracking.CarrierPhaseShift);

		if (bitsync.NumberOfCorrelatorOutInBuffer ==...
		 		bitsync.CorrelatorOutBufferSize)
			fprintf(1, 'Buffer is full, ok...\n');
			if (bitsync.checkFrequencyTimeShiftStd(...
										tracking.FrequencyShiftStdThresholdFine,...
										tracking.TimeShiftStdThresholdFine ))

				tracking.setState('Fine');
				tracking.setFrequencyShiftFilter(LoopOrderFrequencyFine, ...
																				 BandwidthFrequencyFine);
				tracking.setTimeShiftFilter(LoopOrderTimeFine, ...
																		BandwidthTimeFine);
				tracking.setCarrierPhaseShiftFilter(LoopOrderCarrierPhaseFine, ...
																						BandwidthCarrierPhaseFine);
				fprintf(1, 'Time and frequency thresholds ok...\n');
				insync = bitsync.checkBitSynchronization();
				if (insync(1))
					fprintf(1, 'In sync ok! \n');
					if (bitsync.checkCarrierPhaseShiftStdThreshold(...
							tracking.CarrierPhaseShiftStdThreshold))
						fprintf(1, 'Carrier phase threshold ok...\n');
						bitsync.storeBitSoftInformation();
					end
				end
			else
				fprintf(1, 'Above std. thresholds! \n');
			end		
		end

		TimeShiftMem(i) = tracking.TimeShift;
		FrequencyShiftMem(i) = tracking.FrequencyShift;
		CarrierPhaseShiftMem(i) = tracking.CarrierPhaseShift;
		CorrelatorOutMem(:,i) = tracking.CorrelatorOut.';
		ThresholdMem(:,i) = bitsync.Threshold.';
		SuffStatMem(:,i) = bitsync.SufficientStatistics.';

	end

	c = 3e8;
	t = (1:NumberOfCorrelations)*tracking.IntegrationTime;

	figure(1);clf;
	plot(t, TimeShiftMem*c);
	xlabel('Time [s]');
	ylabel('TimeShift [m]');
	
	figure(2);clf;
	plot(t, FrequencyShiftMem/samples.CarrierFrequency*c);
	xlabel('Time [s]');
	ylabel('FrequencyShift [m/s]');

	switch (n)
		case 3
			E = abs(CorrelatorOutMem(1,:));
			P = abs(CorrelatorOutMem(2,:));
			Preal = real(CorrelatorOutMem(2,:));
			Pimag = imag(CorrelatorOutMem(2,:));
			L = abs(CorrelatorOutMem(3,:));
		case 6
			E = abs(CorrelatorOutMem(1,:)) + abs(CorrelatorOutMem(4,:));
			P = abs(CorrelatorOutMem(2,:)) + abs(CorrelatorOutMem(5,:));
			Preal = zeros(2, length(t));
			Preal(1,:) = real(CorrelatorOutMem(2,:));
			Pimag(1,:) = imag(CorrelatorOutMem(2,:));
			Preal(2,:) = real(CorrelatorOutMem(5,:));
			Pimag(2,:) = imag(CorrelatorOutMem(5,:));
			L = abs(CorrelatorOutMem(3,:)) + abs(CorrelatorOutMem(6,:));
		otherwise
			error('Too many correlator outputs')
	end
	figure(3);clf;
	plot(t,	E, t, P, t, L);
	xlabel('Time [s]');
	ylabel('CorrelatorOut[-]');
	legend('Early', 'Prompt', 'Late');


	[p S] = polyfit(t, TimeShiftMem, 2);
	y = polyval(p, t);
	TimeShiftStd = TimeShiftMem - y;
	[p S] = polyfit(t, FrequencyShiftMem, 2);
	y = polyval(p, t);
	FrequencyShiftStd = FrequencyShiftMem - y;
	[p S] = polyfit(t, P, 2);
	y = polyval(p, t);
	PStd = P - y;

	figure(4);clf;
	plot(t,	TimeShiftStd*c);
	xlabel('Time [s]');
	ylabel('TimeShiftStd [m]');

	figure(5);clf;
	plot(t,	FrequencyShiftStd*c/samples.CarrierFrequency);
	xlabel('Time [s]');
	ylabel('FrequencyShiftStd [m/s]');

	figure(6);clf;
	plot(t,	CarrierPhaseShiftMem);
	xlabel('Time [s]');
	ylabel('CarrierPhaseShift [rad]');

	figure(7);clf;
	plot(t,	PStd);
	xlabel('Time [s]');
	ylabel('CorrelatorOutStd [-]');

	for l = 1:(n/3)
		figure(7+l);clf;
		plot(t,	SuffStatMem(l,:), t, ThresholdMem(l,:));
		xlabel('Time [s]');
		ylabel('SufficientStatistics [-]');
		legend('SuffStat', 'Threshold');
	end

	for l = 1:(n/3)
		figure(9+l); clf;
		plot(t, Preal(l,:), t, Pimag(l,:));
		xlabel('Time [s]');
		ylabel('CorrelatorOutMem Prompt [-]');
		legend('Real', 'Imag');
	end

else
	fprintf(1, 'SVID = %d not acquired.\n', SVID);
end

